
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>

#include "rp2040.h"
#include "drvs/sync.h"
#include "pico.h"
#include "pico/multicore.h"
#include "umsg.h"



/*
 */
typedef  struct _tag_rgmsg
{
    io_rw_32  head;
    io_rw_32  tail;
    uint32_t  mask;
    uint32_t  esiz;

    /**/
    uint8_t  buf[4];
        
} rgmsg_t;


int  rgmsg_alloc( uint32_t esiz, uint32_t tlen, rgmsg_t ** pret )
{
    rgmsg_t * prb;

    /* check tlen, 2^n */
    if ( 0 != (tlen & (tlen - 1)) )
    {
        return 1;
    }
    
    /**/
    prb = (rgmsg_t *)malloc( sizeof(rgmsg_t) + (tlen * esiz) );
    if ( NULL == prb )
    {
        return 2;
    }

    /**/
    prb->head = prb->tail = 0;
    prb->mask = tlen - 1;
    prb->esiz = esiz;
    
    /**/
    *pret = prb;
    return 0;
}


/* valid data uint32 numbers */
static inline uint32_t rgmsg_get_elems( rgmsg_t * prb )
{
    return ( prb->head - prb->tail ) & prb->mask;
}


/* unused uint32 spaces */
static inline uint32_t rgmsg_get_frees( rgmsg_t * prb )
{
    return prb->mask - ( ( prb->head - prb->tail ) & prb->mask );
}


/*
data -> ring
return : tlen is ok; other fail.
*/
static inline int rgmsg_enqueue( rgmsg_t * prb, size_t tlen, void * pdat )
{
    size_t  frees;
    uint32_t  temp;
    
    /**/
    frees = (size_t)(prb->mask - (prb->head - prb->tail));
    if ( frees <= 0 )
    {
        return 1;
    }

    /**/
    if ( tlen > prb->esiz )
    {
        tlen = prb->esiz;
    }
    
    /* write entry */
    temp = prb->head;
    temp = (temp & prb->mask) * prb->esiz;
    memcpy( &(prb->buf[temp]), pdat, tlen );
    
    /**/
    prb->head = prb->head + 1;
    return 0;
    
}


/*
ring -> data
*/
static inline int rgmsg_dequeue( rgmsg_t * prb, size_t tlen, void * pdat )
{
    size_t  entrys;
    uint32_t  temp;
    
    /**/
    entrys = (size_t)((prb->head - prb->tail) & prb->mask);
    if ( entrys <= 0 )
    {
        /* noting in ring */
        return -1;
    }

    /**/
    if ( tlen > prb->esiz )
    {
        tlen = prb->esiz;
    }

    /**/
    temp = prb->tail;
    temp = (temp & prb->mask) * prb->esiz;
    memcpy( pdat, &(prb->buf[temp]), tlen );
    
    /**/
    prb->tail = prb->tail + 1;
    return 0;
    
}



/*
每个 entry 是 16 个字节, 也是 4 个 word .
深度当前是 32, 我们系统基本并发, 所以深度要求不高.
当前主要用途是 interrupt >> user 方向传递消息:
a) usb 接口, 从 host 发送过来的消息, 已经转换成 umsg_t 结构.
b) pio 接口, 通过中断和FIFO返回的读写操作结果.

生产者 都运行在中断处理环境中, 中断处理中默认是中断关闭, 不支持中断优先级抢占和嵌套.
消费者 运行在 user 环境中, 因为 user 环境是单循环没有多任务.
所以能做到 单生产者 和 单消费者, 不需要加锁来保证互斥.
*/

static rgmsg_t * sysmsg_ring;


/*
tcnt 的取值范围是 1-4.
返回值: 0 表示成功.
*/
int sysmsg_send_to_user( int tcnt, uint32_t * pary )
{
    return rgmsg_enqueue( sysmsg_ring, (tcnt << 2), pary );
}

/*
tcnt 的取值范围是 1-4.
返回值: 0 表示成功.
*/
int symsg_recv_from_system( int tcnt, uint32_t * pary )
{
    return rgmsg_dequeue( sysmsg_ring, (tcnt << 2), pary );
}



/*
 * usb 最大消息长度是 64 字节.
 * 固定长度 64 + 8 = 72 bytes, 固定数量的内存分配池.
 * 支持 2 个 cpu 同时使用,  使用 spinlock 互斥保护.
 * 单链表保存空闲节点.
 */

static umsg_t * phead;
static umsg_t * ptail;
static spin_lock_t * plock;
uint32_t msg_pool_stats = 0;


umsg_t * umsg_alloc( void )
{
    uint32_t saves;
    umsg_t * pnd;

    saves = spin_lock_blocking( plock );

    if ( phead == NULL ) {
        spin_unlock( plock, saves );
        return NULL;
    }

    /**/
    pnd = phead;
    msg_pool_stats -= 1;
    
    if ( phead == ptail ) {
        phead = NULL;
        ptail = NULL;
        spin_unlock( plock, saves );
        return pnd;
    }
    
    /**/
    phead = phead->next;
    spin_unlock( plock, saves );
    return pnd;
}


void umsg_free( umsg_t * pnd )
{
    uint32_t saves;

    /**/
    pnd->next = NULL;

    saves = spin_lock_blocking( plock );

    /**/
    if ( ptail != NULL ) {

        ptail->next = pnd;
        ptail = pnd;

    } else {
        phead = pnd;
        ptail = pnd;
    }

    msg_pool_stats += 1;
    spin_unlock( plock, saves );
    return;
}



/*
 * 不属于 usb 协议栈的一部分, 仅仅处理消息传递, 
 * 从 usb 传来的消息, 入队列, 通知用户代码, 用户代码再出队列之后使用.
 * 从用户代码发出来, 需要上传 host 的消息, 先入队列, 然后 usb buf 空闲的时候发送给 usb 协议栈.
 * 作为用户代码跟 usb 之间的一个桥梁.
 *
 * usb --> user 初步方案:
 *   user 使用 wfi 唤醒?
 *   队列使用单链表, 无锁.
 */


static uqueue_t me2usb;

int umsg_inp_cbk( void * parg, uint8_t * ptr, int tmax )
{
    int mlen;
    umsg_t * pmsg;

    // nothing to send ..
    pmsg = umsg_q_deque( &me2usb );
    if ( pmsg == NULL ) {
        return 0;
    }

    // copy and free
    mlen = pmsg->mlen;
    memcpy( ptr, pmsg->data, mlen );
    umsg_free( pmsg );

    //
    return mlen;
}


// static uqueue_t usb2me;

int umsg_outp_cbk( void * parg, uint8_t * ptr, int tsiz )
{
    umsg_t * pmsg;
    uint32_t tary[2];

    if ( tsiz <= 0 ) {
        // nothing...
        return 0;
    }

    pmsg = umsg_alloc();
    if ( pmsg == NULL ) {
        printf("alloc null, outp\n");
        return 0;
    }

    // copy from usb dpram.
    pmsg->mlen = (uint8_t)tsiz;
    memcpy( pmsg->data, ptr, tsiz );

    
    if ( (ptr[0] & 0x80) == 0x0 ) {
        
        // send to cpre-0, queue
        tary[0] = SYS_UMSG;
        tary[1] = (uint32_t)(intptr_t)pmsg;
        sysmsg_send_to_user( 2, tary );

    } else {
        // send to cpre-1, through Inter-processor FIFOs (Mailboxes)
        multicore_fifo_push_blocking( (uint32_t)(intptr_t)pmsg );
    }
    
    return 0;
}


extern int ttusb_intf_send( void * pifc, int idx, int tlen, void * pdat );

void umsg_send_to_host( void * pifc, int idx, umsg_t * pmsg )
{
    int iret;

    assert( pmsg->mlen <= 64 );

    /* me2usb queue not empty ??? */
    if ( true != umsg_q_is_empty( &me2usb ) ) {
        umsg_q_enque( &me2usb, pmsg );
        return;
    }

    /* send to host? */
    iret = ttusb_intf_send( NULL, idx, pmsg->mlen, pmsg->data );
    if ( iret == 0 ) {
        umsg_free( pmsg );
        return;
    }

    /* add to queue */
    umsg_q_enque( &me2usb, pmsg );
    return;
}


#include "structs/sio.h"

void FIFO_P0_Handler( void )
{
    umsg_t * pmsg;

    while ( 0 != (sio_hw->fifo_st & SIO_FIFO_ST_VLD_BITS) ) {

        pmsg = (umsg_t *)(uintptr_t) sio_hw->fifo_rd;

        //printf("fifo irq, %p\n", pmsg );
        umsg_send_to_host( NULL, 0, pmsg );
    }
    
    // clear other error flag.
    sio_hw->fifo_st = 0xff;
}



extern int utchan_svc_send( umsg_t * pmsg );

/*
 * 在 core 0 上运行, 当前处理 umsg 的 user 层面交互.
 * 好处是 会自动关闭中断? 代码隔离?...
 */
uintptr_t SVC_umsg_handler( int sid, uintptr_t arg0, uintptr_t regs[] )
{
    uintptr_t uret = 0;

    switch ( sid )  {

    case 3:
        umsg_send_to_host( NULL, 0, (umsg_t *)arg0 );
        break;
        
    default:
        printf("scall, %d\n", sid );
        break;
    }

    return uret;
}



int umsg_init( int num )
{
    int i;
    umsg_t * pmsg;

    /**/
    phead = NULL;
    ptail = NULL;
    plock = spin_lock_init( PICO_SPINLOCK_ID_OS1 );

    /**/
    pmsg = (umsg_t *)malloc( num * sizeof(umsg_t) );
    if ( pmsg == NULL ) {
        return 101;
    }

    /**/
    for ( i=0; i<num; i++ ) {
        umsg_free( pmsg );
        pmsg = pmsg + 1;
    }


    /**/
    i = rgmsg_alloc( 16, 32, &sysmsg_ring );
    if ( i != 0 ) {
        return 102;
    }

    /**/
    umsg_q_init( &me2usb );
    return 0;
}







